#include "rtp/SeqManager.h"
#include "utils.h"

#include <iterator>

namespace Rtp {
template<typename T, uint8_t N>
bool SeqManager<T, N>::SeqLowerThan::operator()(const T& lhs, const T& rhs) const {
    return Utils::Number<T, N>::IsLowerThan(lhs, rhs);
}

template<typename T, uint8_t N>
bool SeqManager<T, N>::SeqHigherThan::operator()(const T& lhs, const T& rhs) const {
    return Utils::Number<T, N>::IsHigherThan(lhs, rhs);
}

template<typename T, uint8_t N>
bool SeqManager<T, N>::IsSeqHigherThan(T lhs, T rhs) {
    return Utils::Number<T, N>::IsHigherThan(lhs, rhs);
}

template<typename T, uint8_t N>
bool SeqManager<T, N>::IsSeqLowerThan(T lhs, T rhs) {
    return Utils::Number<T, N>::IsLowerThan(lhs, rhs);
}

template<typename T, uint8_t N>
SeqManager<T, N>::SeqManager(T initialOutput)
    : initialOutput(initialOutput) {}

template<typename T, uint8_t N>
void SeqManager<T, N>::Sync(T input) {

    this->base = (this->maxOutput - input) & SeqManager::MaxValue;

    this->maxInput = input;
    this->dropped.clear();
}

template<typename T, uint8_t N>
void SeqManager<T, N>::Drop(T input) {
    if (SeqManager<T, N>::IsSeqHigherThan(input, this->maxInput)) {
        this->maxInput = input;
        this->dropped.insert(this->dropped.end(), input);

        ClearDropped();
    }
}

template<typename T, uint8_t N>
bool SeqManager<T, N>::Intput(T input, T& output) {

    auto base = this->base;

    if (this->dropped.empty()) {
        goto done;
    } else {
        if (this->started && SeqManager<T, N>::IsSeqHigherThan(input, this->maxInput)) {
            this->maxInput = input;
        }
        ClearDropped();
        base = this->base;
    }

    if (this->dropped.empty()) {
        goto done;
    } else if (this->dropped.find(input) != this->dropped.end()) {
        return false;
    } else {
        auto droppedCount = this->dropped.size();

        auto it = this->dropped.lower_bound(input);

        droppedCount -= std::distance(it, this->dropped.end());

        base = (base - droppedCount) & SeqManager::MaxValue;
    }
done:
    output = (input + base) & SeqManager::MaxValue;
    if (!this->started) {
        this->started = true;
        this->maxInput = input;
        this->maxOutput = output;
    } else {
        if (SeqManager<T, N>::IsSeqHigherThan(input, this->maxInput)) {
            this->maxInput = input;
        }

        if (SeqManager<T, N>::IsSeqHigherThan(output, this->maxOutput)) {
            this->maxOutput = output;
        }
    }

    output = (output + this->initialOutput) & SeqManager::MaxValue;
    return true;
}

template<typename T, uint8_t N>
T SeqManager<T, N>::GetMaxInput() const {
    return this->maxInput;
}

template<typename T, uint8_t N>
T SeqManager<T, N>::GetMaxOutput() const {
    return this->maxOutput;
}

template<typename T, uint8_t N>
void SeqManager<T, N>::ClearDropped() {
    if (this->dropped.empty()) {
        return;
    }
    const size_t previousDroppedSize = this->dropped.size();

    for (auto it = this->dropped.begin(); it != this->dropped.end();) {

        auto value = *it;
        if (SeqManager<T, N>::IsSeqHigherThan(value, this->maxOutput)) {
            it = this->dropped.erase(it);
        } else {
            break;
        }
    }

    this->base = (this->base - (previousDroppedSize - this->dropped.size())) & SeqManager::MaxValue;
}

// Explicit instantiation to have all SeqManager definitions in this file.
	template class SeqManager<uint8_t>;      // For codecs.
	template class SeqManager<uint8_t, 3>;   // For testing.
	template class SeqManager<uint16_t>;     // For RTP sequence numbers.
	template class SeqManager<uint16_t, 15>; // For PictureID (15 bits).
} // namespace Rtp